/**
 * Quaternion and Vector classes
 */
package rj.cometome;

import android.util.Log;

class Quaternion3d {

	  private float x;
	  private float y;
	  private float z;
	  private float w;

	  private static final float QUATERNION_TRACE_ZERO_TOLERANCE = 0.1f;

	  public void print(String tag) {
		  String str = "X: " + x + " Y: " + y + " Z: " + z + " W: " + w;
		  Log.d(tag, str);
	  }
	  
	  private Quaternion3d() {
	  }

	  public Quaternion3d(float x, float y, float z, float w) {
	    this.x = x;
	    this.y = y;
	    this.z = z;
	    this.w = w;
	  }

	  public Quaternion3d clone() {
	    return new Quaternion3d(x, y, z, w);
	  }

	  public static Quaternion3d identity() {
	    return new Quaternion3d(0, 0, 0, 1);
	  }

	  public static Vector3d rotateVector(Quaternion3d q, Vector3d v) {
		  float k0 = q.w * q.w - 0.5f;
		  float k1;
		  float rx,ry,rz;
		  
		  k1 = v.x * q.x;
		  k1 += v.y * q.y;
		  k1 += v.z * q.z;
		  
		  rx = v.x * k0 + q.x * k1;
		  ry = v.y * k0 + q.y * k1;
		  rz = v.z * k0 + q.z * k1;
		  
		  rx += rx;
		  ry += ry;
		  rz += rz;
		  
		  return new Vector3d(rx,ry,rz);
	  }
	  
	  public static void normalize(Quaternion3d quaternion) {
	    float magnitude = (float) Math
	        .sqrt(((quaternion.x * quaternion.x)
	            + (quaternion.y * quaternion.y)
	            + (quaternion.z * quaternion.z) + (quaternion.w * quaternion.w)));

	    if (magnitude != 0) {
	      quaternion.x /= magnitude;
	      quaternion.y /= magnitude;
	      quaternion.z /= magnitude;
	      quaternion.w /= magnitude;
	    }
	  }

	  public static Quaternion3d fromMatrix(float[] matrix) {
	    Quaternion3d quat = new Quaternion3d();

	    // Trace of diagonal
	    float trace = matrix[0] + matrix[5] + matrix[10];

	    if (trace > 0.0f) {
	      float s = (float) Math.sqrt(trace + 1.0f);
	      quat.w = s * 0.5f;
	      s = 0.5f / s;

	      quat.x = (matrix[9] - matrix[6]) * s;
	      quat.y = (matrix[2] - matrix[8]) * s;
	      quat.z = (matrix[4] - matrix[1]) * s;

	      return quat;
	    }

	    if (matrix[0] > matrix[5]) {
	      if (matrix[10] > matrix[0]) {
	        if (!option2(matrix, quat)) {
	          if (!option1(matrix, quat)) {
	            option3(matrix, quat);
	          }
	        }
	      } else {
	        if (!option1(matrix, quat)) {
	          if (!option2(matrix, quat)) {
	            option3(matrix, quat);
	          }
	        }
	      }
	    } else {
	      if (!option3(matrix, quat)) {
	        if (!option2(matrix, quat)) {
	          option1(matrix, quat);
	        }
	      }
	    }
	    return quat;
	  }

	  private static boolean option1(float[] matrix, Quaternion3d quat) {
	    float s = (float) Math
	        .sqrt(matrix[0] - (matrix[5] + matrix[10]) + 1.0f);
	    if (s > QUATERNION_TRACE_ZERO_TOLERANCE) {
	      quat.x = s * 0.5f;
	      s = 0.5f / s;
	      quat.w = (matrix[9] - matrix[6]) * s;
	      quat.y = (matrix[1] + matrix[4]) * s;
	      quat.z = (matrix[2] + matrix[8]) * s;
	      return true;
	    }
	    return false;
	  }

	  private static boolean option2(float[] matrix, Quaternion3d quat) {
	    float s = (float) Math
	        .sqrt(matrix[10] - (matrix[0] + matrix[5]) + 1.0f);
	    if (s > QUATERNION_TRACE_ZERO_TOLERANCE) {
	      quat.z = s * 0.5f;
	      s = 0.5f / s;
	      quat.w = (matrix[4] - matrix[1]) * s;
	      quat.x = (matrix[8] + matrix[2]) * s;
	      quat.y = (matrix[9] + matrix[6]) * s;
	      return true;
	    }
	    return false;
	  }

	  private static boolean option3(float[] matrix, Quaternion3d quat) {
	    float s = (float) Math
	        .sqrt(matrix[5] - (matrix[10] + matrix[0]) + 1.0f);
	    if (s > QUATERNION_TRACE_ZERO_TOLERANCE) {
	      quat.y = s * 0.5f;
	      s = 0.5f / s;
	      quat.w = (matrix[2] - matrix[8]) * s;
	      quat.z = (matrix[6] + matrix[9]) * s;
	      quat.x = (matrix[4] + matrix[1]) * s;
	      return true;
	    }
	    return false;
	  }

	  public static void setMatrixFromQuaternion(float[] matrix, Quaternion3d quat) {
	    matrix[0] = 1.0f - (2.0f * ((quat.y * quat.y) + (quat.z * quat.z)));
	    matrix[1] = 2.0f * ((quat.x * quat.y) - (quat.z * quat.w));
	    matrix[2] = 2.0f * ((quat.x * quat.z) + (quat.y * quat.w));
	    matrix[3] = 0.0f;
	    matrix[4] = 2.0f * ((quat.x * quat.y) + (quat.z * quat.w));
	    matrix[5] = 1.0f - (2.0f * ((quat.x * quat.x) + (quat.z * quat.z)));
	    matrix[6] = 2.0f * ((quat.y * quat.z) - (quat.x * quat.w));
	    matrix[7] = 0.0f;
	    matrix[8] = 2.0f * ((quat.x * quat.z) - (quat.y * quat.w));
	    matrix[9] = 2.0f * ((quat.y * quat.z) + (quat.x * quat.w));
	    matrix[10] = 1.0f - (2.0f * ((quat.x * quat.x) + (quat.y * quat.y)));
	    matrix[11] = 0.0f;
	    matrix[12] = 0.0f;
	    matrix[13] = 0.0f;
	    matrix[14] = 0.0f;
	    matrix[15] = 1.0f;
	  }

	  public static Quaternion3d fromAxisAndAngle(Vector3d axis, float angle) {
	    Quaternion3d quat = new Quaternion3d();

	    angle *= 0.5f;
	    
	    Vector3d.normalize(axis);
	    
	    float sinAngle = (float) Math.sin(angle);
	    quat.x = (axis.x * sinAngle);
	    quat.y = (axis.y * sinAngle);
	    quat.z = (axis.z * sinAngle);
	    quat.w = (float) Math.cos(angle);

	    return quat;
	  }

	  public static float extractAxisAndAngle(Quaternion3d quat, Vector3d axis) {
	    normalize(quat);
	    float s = (float) Math.sqrt(1.0f - (quat.w * quat.w));
	    if (Math.abs(s) < 0.0005f) {
	      s = 1.0f;
	    }

	    if (axis != null) {
	      axis.x = (quat.x / s);
	      axis.y = (quat.y / s);
	      axis.z = (quat.z / s);
	    }

	    return (float) (Math.acos(quat.w) * 2.0f); // return angle as float
	  }

	  public static Quaternion3d multiply(Quaternion3d quat1, Quaternion3d quat2) {
	    Vector3d v1 = new Vector3d(quat1.x, quat1.y, quat1.z);
	    Vector3d v2 = new Vector3d(quat2.x, quat2.y, quat2.z);

	    float angle = (quat1.w * quat2.w) - Vector3d.dotProduct(v1, v2);

	    Vector3d cp = Vector3d.crossProduct(v1, v2);

	    v1.x *= quat2.w;
	    v1.y *= quat2.w;
	    v1.z *= quat2.w;

	    v2.x *= quat1.w;
	    v2.y *= quat1.w;
	    v2.z *= quat1.w;

	    return new Quaternion3d(v1.x + v2.x + cp.x, v1.y + v2.y + cp.y, v1.z
	        + v2.z + cp.z, angle);
	  }

	  public static void invert(Quaternion3d quat) {
	    float length = 1.0f / ((quat.x * quat.x) + (quat.y * quat.y)
	        + (quat.z * quat.z) + (quat.w * quat.w));
	    quat.x *= -length;
	    quat.y *= -length;
	    quat.z *= -length;
	    quat.w *= length;
	  }

	  public static Quaternion3d fromEulerAngles(float x, float y, float z) {
	    Vector3d vx = new Vector3d(1.f, 0.f, 0.f);
	    Vector3d vy = new Vector3d(0.f, 1.f, 0.f);
	    Vector3d vz = new Vector3d(0.f, 0.f, 1.f);

	    Quaternion3d qx = fromAxisAndAngle(vx, x);
	    Quaternion3d qy = fromAxisAndAngle(vy, y);
	    Quaternion3d qz = fromAxisAndAngle(vz, z);

	    Quaternion3d temp = multiply(qx, qy);
	    return multiply(temp, qz);
	  }

	  public static float dotProduct(Quaternion3d quat1, Quaternion3d quat2) {
	    return quat1.x * quat2.x + quat2.y * quat2.y + quat1.z * quat2.z
	        + quat1.w * quat2.w;
	  }

	  public static Quaternion3d slerp(Quaternion3d start, Quaternion3d finish,
	      float progress) {
	    float startWeight, finishWeight;
	    float difference = (start.x * finish.x) + (start.y * finish.y)
	        + (start.z * finish.z) + (start.w * finish.w);
	    if (1f - Math.abs(difference) > .01f) {
	      float theta = (float) Math.acos(Math.abs(difference));
	      float oneOverSinTheta = (float) (1.f / Math.sin(theta));
	      startWeight = (float) (Math.sin(theta * (1.f - progress)) * oneOverSinTheta);
	      finishWeight = (float) (Math.sin(theta * progress) * oneOverSinTheta);
	      if (difference < 0f) {
	        startWeight = -startWeight;
	      }
	    } else {
	      startWeight = (1.f - progress);
	      finishWeight = progress;
	    }

	    Quaternion3d ret = new Quaternion3d();
	    ret.x = (start.x * startWeight) + (finish.x * finishWeight);
	    ret.y = (start.y * startWeight) + (finish.y * finishWeight);
	    ret.z = (start.z * startWeight) + (finish.z * finishWeight);
	    ret.w = (start.w * startWeight) + (finish.w * finishWeight);
	    normalize(ret);
	    return ret;
	  }
	}

	class Vector3d {
	  public float x;
	  public float y;
	  public float z;

	  private Vector3d() {
	  }

	  public Vector3d(float x, float y, float z) {
	    this.x = x;
	    this.y = y;
	    this.z = z;
	  }

	  public float[] toFloatArray3() {
	    return new float[] { x, y, z };
	  }

	  public float[] toFloatArray4() {
	    return new float[] { x, y, z, 1f };
	  }

	  public void updateFromArray(float[] tab) {
	    x = tab[0];
	    y = tab[1];
	    z = tab[2];
	  }

	  public static Vector3d empty() {
	    return new Vector3d();
	  }

	  public static float magnitude(Vector3d vector) {
	    return (float) Math.sqrt((vector.x * vector.x) + (vector.y * vector.y)
	        + (vector.z * vector.z));
	  }

	  public static void normalize(Vector3d vector) {
	    float vecMag = magnitude(vector);
	    if (vecMag == 0.0f) {
	      vector.x = 1.0f;
	      vector.y = 0.0f;
	      vector.z = 0.0f;
	      return;
	    }
	    vector.x /= vecMag;
	    vector.y /= vecMag;
	    vector.z /= vecMag;
	  }

	  public static float dotProduct(Vector3d vector1, Vector3d vector2) {
	    return vector1.x * vector2.x + vector1.y * vector2.y + vector1.z
	        * vector2.z;
	  }

	  public static Vector3d crossProduct(Vector3d vector1, Vector3d vector2) {
	    Vector3d ret = new Vector3d();
	    ret.x = (vector1.y * vector2.z) - (vector1.z * vector2.y);
	    ret.y = (vector1.z * vector2.x) - (vector1.x * vector2.z);
	    ret.z = (vector1.x * vector2.y) - (vector1.y * vector2.x);
	    return ret;
	  }

	  public Vector3d clone() {
	    return new Vector3d(x, y, z);
	  }

	  public static float distance(Vector3d a, Vector3d b) {
	    float dx = a.x - b.x;
	    float dy = a.y - b.y;
	    float dz = a.z - b.z;
	    return (float) Math.sqrt(dx * dx + dy * dy + dz * dz);
	  }

	}
